#include <cstdint>
#include <iostream>

using std::cout,std::endl;

#define  UDS_X1  0xD1
#define  UDS_X2  0x31
#define  UDS_X3  0x54
#define  UDS_X4  0x07

unsigned char uds_seed[4] = {0,0,0,0,} ;
unsigned char uds_cal[4]   = {0,0,0,0,};
unsigned char uds_key[4]  = {0,0,0,0,};
unsigned char uds_xor[4]   = {UDS_X1,UDS_X2,UDS_X3,UDS_X4,};
int  Uds_Security_Req_Flag = 0;
int  Uds_Security_Level = 0;  //安全级别
int  Uds_Security_Level_Req = 0;

//获取随机数
// void get_seed(void) 
// {
//    uds_seed[0] = (CCU61_T13&0xFF);
//    uds_seed[1] = (CCU61_T13+0xAAAA);
//    uds_seed[2] = (CCU61_T13+0x5555);
//    uds_seed[3] = (CCU61_T13&0xFF00)>>8;
// }

//计算密钥
void uds_key_seed_cal(void) 
{
  int i;
  for(i=0; i<4; i++)  {
     uds_cal[i] = uds_seed[i]^uds_xor[i];
  }
  uds_key[0] = ( (uds_cal[0]&0x0F)<<4) | (uds_cal[1]&0xF0);
  uds_key[1] = ( (uds_cal[1]&0x0F)<<4) | ((uds_cal[2]&0xF0)>>4);
  uds_key[2] = ( (uds_cal[2]&0xF0)) | ((uds_cal[3]&0xF0)>>4);
  uds_key[3] = ( (uds_cal[3]&0x0F)<<4) | (uds_cal[0]&0x0F);
}

//计算密钥
void uds_key_seed_cal1(void)
{
    unsigned char uds_seed1[4] = {0x02,0xb3,0x66,0x46,} ;
    //unsigned char far ECU_Serial_Number[16]="FB010001YUBEIEPS";
    long sum=0;
    long delta=0x9E3779B9;
    char num_rounds=2;   
    unsigned  long V0,V1,K[4]={0,0,0,0};
    long SA_constant_1[4] = {0,0,0,0};  //{0x11,0x22,0x33,0x44};{0x33,0x5A,0xB7,0x98};//
    SA_constant_1[0]='I';
    SA_constant_1[1]='E';
    SA_constant_1[2]='P';
    SA_constant_1[3]='S';
    /**/
    V0=((long)uds_seed1[0]<<24)|((long)(uds_seed1[1]&0x00FF)<<16)|((long)(uds_seed1[2]&0xFF)<<8)|((long)(uds_seed1[3]&0xFF));
    V1=~V0;
    
    K[0]=SA_constant_1[0];
    K[1]=SA_constant_1[1];
    K[2]=SA_constant_1[2];
    K[3]=SA_constant_1[3];

    for(num_rounds=0; num_rounds<2; num_rounds++)
    {
        V0 += ((((V1<<4)^(V1>>5))+V1)^(sum+K[sum&3]));
        sum += delta;
        V1 += ((((V0<<4)^(V0>>5))+V0)^(sum+K[(sum>>11)&3]));
    }
    uds_key[0] = (unsigned char)((V0&0xFF000000)>>24);
    uds_key[1] = (unsigned char)((V0&0x00FF0000)>>16);
    uds_key[2] = (unsigned char)((V0&0x0000FF00)>>8);
    uds_key[3] = (unsigned char)(V0&0x000000FF);
}

int main()
{
    uds_key_seed_cal1();
    cout << (int)uds_key[0] << endl;
    cout << (int)uds_key[1] << endl;
    cout << (int)uds_key[2] << endl;
    cout << (int)uds_key[3] << endl;

   unsigned char uds_seed1[4] = {0x02,0xb3,0x66,0x46,} ;
   cout << (((long)uds_seed1[0]<<24)|((long)(uds_seed1[1]&0x00FF)<<16)|((long)(uds_seed1[2]&0xFF)<<8)|((long)(uds_seed1[3]&0xFF))) << endl;

   long SA_constant_1[4] = {0,0,0,0};
   SA_constant_1[0]='I';
   SA_constant_1[1]='E';
   SA_constant_1[2]='P';
   SA_constant_1[3]='S';
   unsigned  long V0,V1,K[4]={0,0,0,0};
   V0=((long)uds_seed1[0]<<24)|((long)(uds_seed1[1]&0x00FF)<<16)|((long)(uds_seed1[2]&0xFF)<<8)|((long)(uds_seed1[3]&0xFF));
   V1=~V0;
   K[0]=SA_constant_1[0];
   K[1]=SA_constant_1[1];
   K[2]=SA_constant_1[2];
   K[3]=SA_constant_1[3];
   cout << V0 << endl;
   cout << V1 << endl;
   cout << K[0] <<endl;
   cout << K[1] <<endl;
   cout << K[2] <<endl;
   cout << K[3] <<endl;


    return 0;
}